/*
 * @Author: ilikara 3435193369@qq.com
 * @Date: 2025-01-04 06:50:56
 * @LastEditors: ilikara 3435193369@qq.com
 * @LastEditTime: 2025-03-13 08:15:10
 * @FilePath: /2k300_smartcar/src/image_cv.cpp
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "image_cv.h"

cv::Mat raw_frame;
cv::Mat grayFrame;
cv::Mat binarizedFrame;
cv::Mat morphologyExFrame;
cv::Mat track;


// std::vector是 C++ 标准库提供的动态数组, 其大小动态可变
std::vector<int> left_line;              // 左边缘列号数组
std::vector<int> right_line;             // 右边缘列号数组
std::vector<int> mid_line;               // 中线列号数组
std::vector<double> left_line_filtered;  // 中线列号数组
std::vector<double> right_line_filtered; // 中线列号数组
std::vector<double> mid_line_filtered;   // 中线列号数组

int line_tracking_width; // 80
int line_tracking_height; // 60


// 主要目的是将彩色图像转换为黑白二值图像，突出道路标记（如车道线）与背景的对比。
cv::Mat image_binerize(cv::Mat &frame)
{
    cv::Mat h_binary;  // H通道二值化结果 可能捕捉黄色车道线的色调差异
    cv::Mat s_binary;  // S通道二值化结果 能识别高饱和度的颜色（如黄色/白色车道线）
    //cv::Mat v_binary;  // V通道二值化结果 可能因光照变化误判（如地面反光）
    cv::Mat final_binary; // 最终二值化结果
    cv::Mat hsvImage;

    cv::cvtColor(frame, hsvImage, cv::COLOR_BGR2HSV); // 将输入的BGR彩色图像转换为HSV颜色空间（色调Hue、饱和度Saturation、亮度Value）

    // 仅处理图像的下半部分
    int height = frame.rows;
    int half_height = height / 2;  // 取下半部分
    //int quarter_height = height / 4;  // 计算 1/4 高度
    //int three_quarter_height = height - quarter_height;  // 计算 3/4 高度

/*----------------------------------------------------------------------------------------------*/
    // 如果要处理整张图像，只需要将下面几句代码注释掉,
    /*int height = frame.rows;
      int half_height = height / 2; 
      cv::Mat full_binary = cv::Mat::zeros(frame.size(), CV_8UC1);
      final_binary.copyTo(full_binary(roi));*/
    //将return full_binary修改为return final_binary
    //将cv::split(hsvLower, hsvChannels);修改为cv::split(hsvImage, hsvChannels);


    // 如果要处理四分之三图像, 只需要将以下代码解除注释
    /*
    int quarter_height = height / 4;  // 计算 1/4 高度
    int three_quarter_height = height - quarter_height;  // 计算 3/4 高度
    cv::Rect roi(0, quarter_height, frame.cols, three_quarter_height);
    cv::Mat hsvLower = hsvImage(roi);  // 仅处理下四分之三部分
    并注释掉:
    int half_height = height / 4;  // 取下半部分    
    cv::Rect roi(0, half_height, frame.cols, half_height);
    cv::Mat hsvLower = hsvImage(roi);  // 仅处理下半部分
    */
/*----------------------------------------------------------------------------------------------*/


    // 创建 ROI（Region of Interest），仅处理下半部分
    cv::Rect roi(0, half_height, frame.cols, half_height);
    cv::Mat hsvLower = hsvImage(roi);  // 仅处理下半部分

    // 创建 ROI（Region of Interest），仅处理下四分之三部分
    //cv::Rect roi(0, quarter_height, frame.cols, three_quarter_height);
    //cv::Mat hsvLower = hsvImage(roi);  // 仅处理下四分之三部分

    // 分离HSV通道（仅对下半部分）
    std::vector<cv::Mat> hsvChannels;
    cv::split(hsvLower, hsvChannels);  // 仅处理下半部分

    // 预处理：高斯模糊降噪
    cv::GaussianBlur(hsvChannels[0], hsvChannels[0], cv::Size(3, 3), 0);
    cv::GaussianBlur(hsvChannels[1], hsvChannels[1], cv::Size(3, 3), 0);
    //cv::GaussianBlur(hsvChannels[2], hsvChannels[2], cv::Size(3, 3), 0);

    // 对H通道和S通道分别进行OTSU自动阈值二值化,OTSU方法会自动计算最佳阈值
    cv::threshold(hsvChannels[0], h_binary, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
    cv::threshold(hsvChannels[1], s_binary, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
    //cv::threshold(hsvChannels[2], v_binary, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
    // 使用自适应阈值（适用于光照不均的情况）
    //cv::adaptiveThreshold(hsvChannels[0], h_binary, 255, cv::ADAPTIVE_THRESH_GAUSSIAN_C, cv::THRESH_BINARY_INV, 11, 2);
    //cv::adaptiveThreshold(hsvChannels[1], s_binary, 255, cv::ADAPTIVE_THRESH_GAUSSIAN_C, cv::THRESH_BINARY_INV, 11, 2);

    //将H通道和S通道的二值化结果进行逻辑或运算合并
    cv::bitwise_or(h_binary, s_binary, final_binary);
    //cv::bitwise_or(final_binary, v_binary, final_binary);

    // 创建全黑图像（上半部分填充黑色）
    cv::Mat full_binary = cv::Mat::zeros(frame.size(), CV_8UC1);

    // 将下半部分的二值化结果复制到最终图像
    final_binary.copyTo(full_binary(roi));

    return full_binary;
    //return final_binary;
}


// find_road 此函数未用到
/*
目的是通过 ​​区域生长（Flood Fill）算法​​ 从二值化图像中提取车道区域，生成一个掩码（Mask），标记出属于道路的像素区域。
输入​​：二值化图像 binarizedFrame（黑白图像，车道线为白色，背景为黑色）。
​​输出​​：掩码图像 outputImage，标记道路区域（值为128）和非道路区域（值为0）。
*/
cv::Mat find_road(cv::Mat &frame) 
{   
    // 仅处理图像的下半部分
    int height = frame.rows;
    int half_height = height / 2;  // 取下半部分
    cv::Rect roi(0, half_height, frame.cols, half_height);
    cv::Mat lower_half = frame(roi).clone();  // 仅处理下半部分（深拷贝）

    // 形态学开运算,去除二值化图像中的细小噪声（如孤立的白色噪点）。
    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(2, 2));  // 使用十字形（MORPH_CROSS）的2x2结构元素
    cv::Mat morphologyExLower;
    cv::morphologyEx(lower_half, morphologyExLower, cv::MORPH_OPEN, kernel); // 开运算（先腐蚀后膨胀），可平滑边缘并断开狭窄的连接。

    // 初始化掩码,创建一个比原图大一圈的掩码（Flood Fill需要额外边界），初始值为0。
    cv::Mat mask = cv::Mat::zeros(half_height + 2, frame.cols + 2, CV_8UC1);

    // 设置种子点.种子点位置​​：图像底部中心（假设车辆摄像头位于车道中央）。
    // 种子点（Seed Point）​​ 是区域生长算法（如Flood Fill）的起始位置，算法从该点开始，根据一定的规则（如颜色相似性）向周围像素扩散，标记或填充符合条件的区域。
    cv::Point seedPoint(frame.cols / 2, half_height - 10);
    cv::circle(morphologyExLower, seedPoint, 10, 255, -1); // 在种子点位置画一个白色实心圆（仅用于调试，实际不影响算法）

    // Flood Fill逻辑​​：从种子点开始，向周围像素扩散，将亮度差异在 [loDiff, upDiff] 范围内的像素标记为 newVal（128）。
    // ​​掩码作用​​：记录哪些像素已被填充（避免重复处理）。
    cv::Scalar newVal(128);  // 填充值为128（道路区域）
    cv::Scalar loDiff(20), upDiff(20);  // 亮度差异范围
    cv::floodFill(morphologyExLower, mask, seedPoint, newVal, 0, loDiff, upDiff, 8);

    // 提取有效区域​​.从掩码中裁剪出与原图大小相同的有效区域（去除多余的边界）。
    cv::Mat outputLower = cv::Mat::zeros(half_height, frame.cols, CV_8UC1);
    mask(cv::Rect(1, 1, frame.cols, half_height)).copyTo(outputLower);

    // 合并结果（上半部分填充0，下半部分为Flood Fill结果）
    cv::Mat outputImage = cv::Mat::zeros(frame.size(), CV_8UC1);
    outputLower.copyTo(outputImage(roi));

    return outputImage;
}


void image_main()
{
    cv::Mat resizedFrame;

    
    // raw_frame是视频流得到的原始图像, 
    cv::resize(raw_frame, resizedFrame, cv::Size(line_tracking_width, line_tracking_height));

    // 二值化
    binarizedFrame = image_binerize(resizedFrame);

    // 将binarizedFrame的数据复制到track变量中，使得后续处理可以直接使用 track进行操作。
    track = (binarizedFrame);

    left_line.clear(); // 清空向量中的所有元素
    right_line.clear();
    mid_line.clear();
    left_line_filtered.clear();
    right_line_filtered.clear();
    mid_line_filtered.clear();

    left_line.resize(line_tracking_height, -1); // 重置大小为行数，初始值-1
    right_line.resize(line_tracking_height, -1);
    mid_line.resize(line_tracking_height, -1);
    left_line_filtered.resize(line_tracking_height, -1);
    right_line_filtered.resize(line_tracking_height, -1);
    mid_line_filtered.resize(line_tracking_height, -1);

    // ​​将OpenCV的二值图像数据track.data重新解释为一个二维数组​​，方便后续通过 IMG[row][col]的方式直接访问像素值。
    // 假设 line_tracking_width = 80，图像尺寸为 80x60（宽x高）
    // 访问第 2 行第 5 列的像素:
    // uchar pixel_value = IMG[1][4];
    uchar(*IMG)[line_tracking_width] = reinterpret_cast<uchar(*)[line_tracking_width]>(track.data);

    // 遍历每一行
    for (int i = 30; i < line_tracking_height; ++i)
    {
        int max_start = -1, max_end = -1;  // 记录当前行最长的白色区域的起止列号
        int current_start = -1;
        int current_length = 0;
        int max_length = 0;

        // 扫描当前行的所有列，寻找连续白色像素块
        for (int j = 0; j < line_tracking_width; ++j) 
        {
            if (IMG[i][j]) // 当前像素是车道线（白色）
            {
                if (current_length == 0) // 记录连续白色区域的起始列
                {
                    current_start = j;
                    current_length = 1;
                }
                else
                {
                    current_length++;
                }
                // 更新最长连续区域的起止位置
                if (current_length >= max_length)
                {
                    max_length = current_length;
                    max_start = current_start;
                    max_end = j;
                }
            }
            else // 当前像素是背景（黑色）
            {
                current_length = 0;
                current_start = -1;
            }
        }
        // 记录当前行的左右边缘
        if (max_length > 0)
        {
            left_line[i] = max_start; // 左边缘 = 最长白色区域的起始列
            right_line[i] = max_end; // 右边缘 = 最长白色区域的结束列
        }
        else
        {
            left_line[i] = -1; // 未检测到车道线
            right_line[i] = -1;
        }
    }



    // 中线计算
    double a = 0.6;
    for (int row = line_tracking_height - 1; row >= 30; --row)
    {
        if (left_line[row] == -1 && right_line[row] == -1) // 左右边线均丢线(即找不到左右边线)
        {
            mid_line[row] = mid_line[row + 1]; // 沿用已经找到的下一行中线
            if (mid_line[row] > line_tracking_width / 2)
            {
                right_line[row] = line_tracking_width - 1;
                left_line[row] = mid_line[row + 1];
            }
            else
            {
                left_line[row] = 0;
                right_line[row] = mid_line[row + 1];
            }
        }
        else
        {
            mid_line[row] = (left_line[row] + right_line[row]) / 2;
        }

        // 通过加权平均的方式平滑数据，减少噪声和突变，使检测到的车道线更稳定。
        // a 是滤波系数，控制当前行与下一行数据的权重。
        // ​​a越大​​：当前行的权重越高，滤波结果更敏感，但噪声抑制能力弱
        // a越小​​：下一行的权重越高，结果更平滑，但响应延迟增加。
        // 实际中用于误差计算的就是mid_line_filtered
        if (row == line_tracking_height - 1) // 最后一行（图像最底部）作为基准行，直接使用原始检测值（无下一行数据可参考）
        {
            left_line_filtered[row] = left_line[row];
            right_line_filtered[row] = right_line[row];
            mid_line_filtered[row] = mid_line[row];
        }
        else
        {
            left_line_filtered[row] = a * left_line[row] + (1 - a) * left_line_filtered[row + 1];
            right_line_filtered[row] = a * right_line[row] + (1 - a) * right_line_filtered[row + 1];
            // mid_line_filtered[row] = a * mid_line[row] + (1 - a) * mid_line_filtered[row + 1];
            mid_line_filtered[row] = (left_line_filtered[row] + right_line_filtered[row]) / 2.0;
        }
    }
}


/*
void image_main()
{
    cv::Mat resizedFrame;

    cv::resize(raw_frame, resizedFrame, cv::Size(line_tracking_width, line_tracking_height));

    binarizedFrame = image_binerize(resizedFrame);

    track = (binarizedFrame);

    left_line.clear(); // 清空向量中的所有元素
    right_line.clear();
    mid_line.clear();
    left_line_filtered.clear();
    right_line_filtered.clear();
    mid_line_filtered.clear();

    left_line.resize(line_tracking_height, -1); // 重置大小为行数，初始值-1
    right_line.resize(line_tracking_height, -1);
    mid_line.resize(line_tracking_height, -1);
    left_line_filtered.resize(line_tracking_height, -1);
    right_line_filtered.resize(line_tracking_height, -1);
    mid_line_filtered.resize(line_tracking_height, -1);

    // 将 OpenCV 的 Mat 图像数据转换为二维数组形式​​，便于直接通过 IMG[height][width] 访问像素值。80X60
    uchar(*IMG)[line_tracking_width] = reinterpret_cast<uchar(*)[line_tracking_width]>(track.data);

    int white_count_mid = 0; // 确定首行中线起始点
    int white_count = 0;
    int black_count = 0;
    bool have_get_left_line = false;
    bool have_get_right_line = false;
    // 先找第一行的中点位置
    // 1/2处
    for(int i = line_tracking_width / 2 - 3; i <= line_tracking_width / 2 + 3; i++)
    {
        if(static_cast<int>(IMG[line_tracking_height - 1][i]) == 255) white_count_mid++;
    }
    // 1/4处
    //for(int i = line_tracking_width / 4 - 3; i <= line_tracking_width / 4 + 3; i++)
    //{
    //    if(static_cast<int>(IMG[line_tracking_height - 1][i]) == 255) white_count_mid++;
    //}
    // 3/4处
    //for(int i = line_tracking_width * 3 / 4 - 3; i <= line_tracking_width * 3 / 4 + 3; i++)
    //{
    //    if(static_cast<int>(IMG[line_tracking_height - 1][i]) == 255) white_count_mid++;
    //}
    printf("white_count_mid:%d", white_count_mid);
    if(white_count_mid >= 5){ 
        for(int i = line_tracking_width / 2; i >= 0; i--) // 从屏幕中心往左找
        {
            for(int j = i; j >= i - 4; j--) // 五个像素点为一组
            {
                if(j >= 4)
                {
                    if(static_cast<int>(IMG[line_tracking_height - 1][j]) == 255) white_count++;
                    else if(static_cast<int>(IMG[line_tracking_height - 1][j]) == 0) black_count++;
                }
                if(white_count >= 2 && black_count >= 2)
                {
                    left_line[0] = i;
                    have_get_left_line = true;
                    break;
                }else{
                    white_count = 0;
                    black_count = 0; 
                }
            }

            if(have_get_left_line == true) // 找到左边线退出循环
            {   
                white_count = 0;
                black_count = 0;
                have_get_left_line = false;
                break;
            }else if(have_get_left_line == false && i == 0) // 未找到左边线
            {
                white_count = 0;
                black_count = 0;
                left_line[0] = 0;
            }
        }
        for(int i = line_tracking_width / 2; i <= line_tracking_width - 1; i++) // 从屏幕中心往右找
        {
            for(int j = i; j <= i + 4; j++) // 五个像素点为一组
            {
                if(j <= line_tracking_width - 6)
                {
                    if(static_cast<int>(IMG[line_tracking_height - 1][j]) == 255) white_count++;
                    else if(static_cast<int>(IMG[line_tracking_height - 1][j]) == 0) black_count++;
                }
                if(white_count >= 2 && black_count >= 2)
                {
                    right_line[0] = i;
                    have_get_right_line = true;
                    break;
                }else{
                    white_count = 0;
                    black_count = 0; 
                }
            }

            if(have_get_right_line == true) // 找到右边线退出循环
            {   
                white_count = 0;
                black_count = 0;
                have_get_right_line = false;
                break;
            }else if(have_get_right_line == false && i == line_tracking_width - 1) // 未找到左边线
            {
                white_count = 0;
                black_count = 0;
                right_line[0] = line_tracking_width - 1;
            }
        }

        mid_line[0] = (int)((right_line[0] + left_line[0]) / 2);
    }

    for(int i = line_tracking_height - 2; i >= line_tracking_height - 41; i -= 2)
    {   
        int idx = (line_tracking_height - 2 - i) / 2;
        for(int j = mid_line[i + 1]; j >= 0; j--) // 从屏幕中心往左找
        {
            for(int k = i; k >= j - 4; k--) // 五个像素点为一组
            {
                if(k >= 4)
                {
                    if(static_cast<int>(IMG[line_tracking_height - 1][k]) == 255) white_count++;
                    else if(static_cast<int>(IMG[line_tracking_height - 1][k]) == 0) black_count++;
                }
                if(white_count >= 2 && black_count >= 2)
                {
                    left_line[idx] = j;
                    have_get_left_line = true;
                    break;
                }else{
                    white_count = 0;
                    black_count = 0; 
                }
            }

            if(have_get_left_line == true) // 找到左边线退出循环
            {   
                white_count = 0;
                black_count = 0;
                have_get_left_line = false;
                break;
            }else if(have_get_left_line == false && j == 0) // 未找到左边线
            {
                white_count = 0;
                black_count = 0;
                left_line[idx] = 0;
            }
        }
        for(int j = mid_line[i + 1]; j <= line_tracking_width - 1; j++) // 从屏幕中心往右找
        {
            for(int k = j; k <= j + 4; k++) // 五个像素点为一组
            {
                if(k <= line_tracking_width - 6)
                {
                    if(static_cast<int>(IMG[line_tracking_height - 1][k]) == 255) white_count++;
                    else if(static_cast<int>(IMG[line_tracking_height - 1][k]) == 0) black_count++;
                }
                if(white_count >= 2 && black_count >= 2)
                {
                    right_line[idx] = j;
                    have_get_right_line = true;
                    break;
                }else{
                    white_count = 0;
                    black_count = 0; 
                }
            }

            if(have_get_right_line == true) // 找到右边线退出循环
            {   
                white_count = 0;
                black_count = 0;
                have_get_right_line = false;
                break;
            }else if(have_get_right_line == false && j == line_tracking_width - 1) // 未找到左边线
            {
                white_count = 0;
                black_count = 0;
                right_line[idx] = line_tracking_width - 1;
            }
        }

        mid_line[idx] = (int)((right_line[idx] + left_line[idx]) / 2);
    }
    std::cout << "mid_line[0]:" << mid_line[0] << std::endl;
    for(int i = 0; i <= 20; i++){
        printf("mid_line[%d]:%d\n", i, mid_line[i]);
    }
}
*/
/*
void image_main()
{
    cv::Mat resizedFrame;

    cv::resize(raw_frame, resizedFrame, cv::Size(line_tracking_width, line_tracking_height));

    binarizedFrame = image_binerize(resizedFrame);

    track = (binarizedFrame);

    left_line.clear(); // 清空向量中的所有元素
    right_line.clear();
    mid_line.clear();
    left_line_filtered.clear();
    right_line_filtered.clear();
    mid_line_filtered.clear();

    left_line.resize(line_tracking_height, -1); // 重置大小为行数，初始值-1
    right_line.resize(line_tracking_height, -1);
    mid_line.resize(line_tracking_height, -1);
    left_line_filtered.resize(line_tracking_height, -1);
    right_line_filtered.resize(line_tracking_height, -1);
    mid_line_filtered.resize(line_tracking_height, -1);

    // 将 OpenCV 的 Mat 图像数据转换为二维数组形式​​，便于直接通过 IMG[height][width] 访问像素值。80X60
    uchar(*IMG)[line_tracking_width] = reinterpret_cast<uchar(*)[line_tracking_width]>(track.data);

    int white_count_mid = 0;
    // 先找第一行的中点位置
    // 1/2处
    for(int i = line_tracking_width / 2 - 3; i <= line_tracking_width / 2 + 3; i++)
    {
        if(static_cast<int>(IMG[line_tracking_height - 1][i]) == 255) white_count_mid++;
    }
    // 1/4处
    //for(int i = line_tracking_width / 4 - 3; i <= line_tracking_width / 4 + 3; i++)
    //{
    //    if(static_cast<int>(IMG[line_tracking_height - 1][i]) == 255) white_count_mid++;
    //}
    // 3/4处
    //for(int i = line_tracking_width * 3 / 4 - 3; i <= line_tracking_width * 3 / 4 + 3; i++)
    //{
    //    if(static_cast<int>(IMG[line_tracking_height - 1][i]) == 255) white_count_mid++;
    //}
    printf("white_count_mid:%d\n", white_count_mid);
    if(white_count_mid >= 5){ 
        for(int i = line_tracking_width / 2; i > 0; i--) // 从屏幕中心往左找
        {   printf("static_cast<int>(IMG[line_tracking_height - 1][i]):%d\n", static_cast<int>(IMG[line_tracking_height - 1][i]));
            if(static_cast<int>(IMG[line_tracking_height - 1][i]) == 255 && static_cast<int>(IMG[line_tracking_height - 1][i - 1]) == 0 && i > 1)
            {   
                left_line[0] = i;
            }else if(i == 1)
            {
                left_line[0] = 1;
            }

        }
        for(int i = line_tracking_width / 2; i < line_tracking_width - 1; i++) // 从屏幕中心往右找
        {

            if(static_cast<int>(IMG[line_tracking_height - 1][i]) == 255 && static_cast<int>(IMG[line_tracking_height - 1][i + 1]) == 0 && i < line_tracking_width - 1)
            {
                right_line[0] = i;
            }else if(i == line_tracking_width - 2)
            {
                right_line[0] = line_tracking_width;
            }
        }
        mid_line[0] = (int)((right_line[0] + left_line[0]) / 2);
    }
    
    for(int i = line_tracking_height - 2; i >= line_tracking_height - 41; i -= 2)
    {   
        int idx = (line_tracking_height - 2 - i) / 2;

        for(int j = mid_line[i + 1]; j > 0; j--) 
        {
            if(static_cast<int>(IMG[i][j]) == 255 && static_cast<int>(IMG[i][j - 1]) == 0 && j > 1)
            {
                left_line[idx] = i;
            }else if(j == 1)
            {
                left_line[idx] = 1;
            }
        }

        for(int j = mid_line[i + 1]; j < line_tracking_width - 1; j++) // 从屏幕中心往右找
        {
            if(static_cast<int>(IMG[i][j]) == 255 && static_cast<int>(IMG[i][j + 1]) == 0 && j < line_tracking_width - 1)
            {
                right_line[idx] = i;
            }else if(j == line_tracking_width - 2)
            {
                right_line[idx] = line_tracking_width;
            }
        }

        mid_line[idx] = (int)((right_line[idx] + left_line[idx]) / 2);
    }
    std::cout << "mid_line[0]:" << mid_line[0] << std::endl;
    for(int i = 0; i <= 20; i++){
        printf("mid_line[%d]:%d\n", i, mid_line[i]);
    }
}*/
